#include "fdcan_interface.h"
#include <string.h>
#include "mjbots.h"

FDCAN_FilterTypeDef sFilterConfig;

FDCAN_TxHeaderTypeDef TxHeader = 
{
  .TxFrameType = FDCAN_DATA_FRAME,
  .ErrorStateIndicator = FDCAN_ESI_ACTIVE,
  .BitRateSwitch = FDCAN_BRS_OFF,
  .FDFormat = FDCAN_FD_CAN,
  .TxEventFifoControl = FDCAN_NO_TX_EVENTS,
  .MessageMarker = 0,
};
FDCAN_RxHeaderTypeDef RxHeader;

uint8_t RxData[20];

can_msg_queue_s can_msg_queue = 
{
    .write_index = 0,
    .read_index = 0,
};

void fdcan_enable(void)
{
		HAL_GPIO_WritePin(CAN_S_GPIO_Port, CAN_S_Pin, GPIO_PIN_RESET);
}

void fdcan_filter_init(FDCAN_HandleTypeDef * fdcanHandle)
{    
    if (HAL_FDCAN_ConfigGlobalFilter(fdcanHandle, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE) != HAL_OK)
    {
        Error_Handler();
    }
    
    if (HAL_FDCAN_ActivateNotification(
                fdcanHandle, FDCAN_IT_RX_FIFO0_NEW_MESSAGE|FDCAN_IT_TX_FIFO_EMPTY, 0) != HAL_OK) {
        Error_Handler();
    }
             
    HAL_FDCAN_ConfigTxDelayCompensation(fdcanHandle, fdcanHandle->Init.DataPrescaler * fdcanHandle->Init.DataTimeSeg1, 0);
    HAL_FDCAN_EnableTxDelayCompensation(fdcanHandle);
                
    if (HAL_FDCAN_Start(fdcanHandle) != HAL_OK) {
        Error_Handler();
    }
}

uint32_t get_fdcan_dlc(uint16_t size)
{
    uint32_t fdcan_dlc = 0;
    if(size <= 8)
	{
		fdcan_dlc = size << 16;
	}
	else if(size == 12)
	{
		fdcan_dlc = FDCAN_DLC_BYTES_12;
	}
	else if(size == 16)
	{
		fdcan_dlc = FDCAN_DLC_BYTES_16;
	}
  else if(size == 20)
	{
		fdcan_dlc = FDCAN_DLC_BYTES_20;
	}
  else if(size == 24)
  {
    fdcan_dlc = FDCAN_DLC_BYTES_24;
  }
  else if(size == 32)
  {
    fdcan_dlc = FDCAN_DLC_BYTES_32;
  }
  else if(size == 48)
  {
    fdcan_dlc = FDCAN_DLC_BYTES_48;
  }
  else if(size == 64)
  {
    fdcan_dlc = FDCAN_DLC_BYTES_64;
  }
  return fdcan_dlc;
}
void fdcan_send(uint32_t id, uint8_t *data, uint16_t size)
{
    TxHeader.Identifier = id;
    if(id > 0x7ff)
    {
        TxHeader.IdType = FDCAN_EXTENDED_ID;
    }
    else
    {
        TxHeader.IdType = FDCAN_STANDARD_ID;
    }
    TxHeader.DataLength = get_fdcan_dlc(size);
    HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &TxHeader, data);
}

uint8_t can_send_flag = 0;
void push_queue(uint32_t id, uint8_t *data, uint16_t size)
{
    if(can_send_flag == 0)  // 未在发送状态
    {
        fdcan_send(id, data, size);
        can_send_flag = 1;  // 标记发送状态
    }
    else                    // 如果有消息在发送，放入消息发送队列
    {
        can_msg_queue.can_msg[can_msg_queue.write_index].id = id;
        can_msg_queue.can_msg[can_msg_queue.write_index].length = size;
        memcpy(can_msg_queue.can_msg[can_msg_queue.write_index].data, data, size);
        can_msg_queue.write_index++;
        if(can_msg_queue.write_index >= CAN_MSG_QUEUE_LENGTH)
        {
            can_msg_queue.write_index = 0;
        }
    }    
}

void pull_queue(void)
{
    if(can_msg_queue.read_index != can_msg_queue.write_index)
    {
        fdcan_send(can_msg_queue.can_msg[can_msg_queue.read_index].id, can_msg_queue.can_msg[can_msg_queue.read_index].data, can_msg_queue.can_msg[can_msg_queue.read_index].length);
        can_msg_queue.read_index++;
        if(can_msg_queue.read_index >= CAN_MSG_QUEUE_LENGTH)
        {
            can_msg_queue.read_index = 0;
        }
    }
    else
    {
        can_send_flag = 0;
    }
}

uint32_t fdcan_recv(void)
{
    
    if(HAL_FDCAN_GetRxMessage(&hfdcan1,FDCAN_RX_FIFO0,&RxHeader,RxData) != HAL_OK)
    {
        return 0;
    }
    parse_mjbots(RxData, RxHeader.Identifier);
    return RxHeader.DataLength;    
}

void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
{
    if(fdcan_recv() != 0)
    {
        HAL_GPIO_TogglePin(LED2_GPIO_Port,LED2_Pin);
    }
}

void HAL_FDCAN_TxFifoEmptyCallback(FDCAN_HandleTypeDef *hfdcan)
{
    pull_queue();
}

